<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
<html xmlns="http://www.w3.org/1999/xhtml">
<head>
<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
<meta http-equiv="X-UA-Compatible" content="IE=9"/>
<meta name="generator" content="Doxygen 1.8.11"/>
<title>Aria: customRangeSensorExample.cpp</title>
<link href="tabs.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="jquery.js"></script>
<script type="text/javascript" src="dynsections.js"></script>
<link href="navtree.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="resize.js"></script>
<script type="text/javascript" src="navtreedata.js"></script>
<script type="text/javascript" src="navtree.js"></script>
<script type="text/javascript">
  $(document).ready(initResizable);
  $(window).load(resizeHeight);
</script>
<link href="doxygen.css" rel="stylesheet" type="text/css" />
</head>
<body>
<div id="top"><!-- do not remove this div, it is closed by doxygen! -->
<div id="titlearea">
<table cellspacing="0" cellpadding="0">
 <tbody>
 <tr style="height: 56px;">
  <td id="projectalign" style="padding-left: 0.5em;">
   <div id="projectname">Aria
   &#160;<span id="projectnumber">2.9.4</span>
   </div>
  </td>
 </tr>
 </tbody>
</table>
</div>
<!-- end header part -->
<!-- Generated by Doxygen 1.8.11 -->
  <div id="navrow1" class="tabs">
    <ul class="tablist">
      <li><a href="index.html"><span>Main&#160;Page</span></a></li>
      <li><a href="pages.html"><span>Related&#160;Pages</span></a></li>
      <li><a href="modules.html"><span>Modules</span></a></li>
      <li><a href="annotated.html"><span>Classes</span></a></li>
      <li><a href="files.html"><span>Files</span></a></li>
      <li><a href="examples.html"><span>Examples</span></a></li>
    </ul>
  </div>
</div><!-- top -->
<div id="side-nav" class="ui-resizable side-nav-resizable">
  <div id="nav-tree">
    <div id="nav-tree-contents">
      <div id="nav-sync" class="sync"></div>
    </div>
  </div>
  <div id="splitbar" style="-moz-user-select:none;" 
       class="ui-resizable-handle">
  </div>
</div>
<script type="text/javascript">
$(document).ready(function(){initNavTree('customRangeSensorExample_8cpp-example.html','');});
</script>
<div id="doc-content">
<div class="header">
  <div class="headertitle">
<div class="title">customRangeSensorExample.cpp</div>  </div>
</div><!--header-->
<div class="contents">
<p>example showing how to implement an <a class="el" href="classArRangeDevice.html" title="The base class for all sensing devices which return range information from the device (mounted on a r...">ArRangeDevice</a> subclass. </p><dl class="section see"><dt>See also</dt><dd>customRangeSensorExampleRobotTask.cpp</dd></dl>
<div class="fragment"><div class="line"></div><div class="line"><span class="preprocessor">#include &quot;Aria.h&quot;</span></div><div class="line"></div><div class="line"><span class="keyword">class </span>ExampleRangeDevice : <span class="keyword">public</span> <span class="keyword">virtual</span> <a name="_a0"></a><a class="code" href="classArRangeDevice.html">ArRangeDevice</a></div><div class="line">{</div><div class="line">  <span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> myCounter;</div><div class="line">  std::vector&lt;ArSensorReading&gt; myReadings;</div><div class="line"><span class="keyword">public</span>:</div><div class="line">  ExampleRangeDevice() : <a name="a1"></a><a class="code" href="classArRangeDevice.html#ae5317e9755d5ef4ddda1acbfd5c79f2f">ArRangeDevice</a>(</div><div class="line">    200,  <span class="comment">// Current buffer size. Choose a value big enough for at least one set of readings</span></div><div class="line">    400,  <span class="comment">// Cumulative buffer size</span></div><div class="line">    <span class="stringliteral">&quot;ExampleRangeDevice&quot;</span>, <span class="comment">// Name</span></div><div class="line">    25000 <span class="comment">// Maximum range limit. Set this to &gt;= sensor&#39;s maximum range</span></div><div class="line">  ),</div><div class="line">    myCounter(0)</div><div class="line">  {</div><div class="line">    <span class="comment">// This simulates something like a scanning laser rangefinder in which a</span></div><div class="line">    <span class="comment">// known, fixed number of rangemeasurements are provided each</span></div><div class="line">    <span class="comment">// at different angles relative to the same origin</span></div><div class="line">    <span class="comment">// point.   we will simulate a sensor placed on the robot 250 mm in front</span></div><div class="line">    <span class="comment">// of the robot&#39;s center point, and we will generate 5 fake readings: every</span></div><div class="line">    <span class="comment">// 9 degrees over a +/-45 degree range.</span></div><div class="line">    <span class="keywordflow">for</span>(<span class="keywordtype">float</span> angle = -45; angle &lt;= 45; angle += 10)</div><div class="line">    {</div><div class="line">      myReadings.push_back(<a name="_a2"></a><a class="code" href="classArSensorReading.html">ArSensorReading</a>(250, 0, angle));</div><div class="line">    }</div><div class="line"></div><div class="line">    <a name="a3"></a><a class="code" href="classArLog.html#a43a8b3789126c818f390f24bdbceccce">ArLog::log</a>(<a name="a4"></a><a class="code" href="classArLog.html#ac8cc0fb3aa323ab2a1c21340fdd1dce3a7040faf60eeb155eaa85d439b1066ca1">ArLog::Normal</a>, <span class="stringliteral">&quot;ExampleRangeDevice: Will provide a set of %d readings each update&quot;</span>, myReadings.size());</div><div class="line"></div><div class="line">    <span class="comment">// configure visualization properties:</span></div><div class="line">    <a name="a5"></a><a class="code" href="classArRangeDevice.html#a7b09eb692d4c9d0f9b51cca41f9e9665">setCurrentDrawingData</a>(<span class="keyword">new</span> <a name="_a6"></a><a class="code" href="classArDrawingData.html">ArDrawingData</a>(<span class="stringliteral">&quot;polyDots&quot;</span>, </div><div class="line">      <a name="_a7"></a><a class="code" href="classArColor.html">ArColor</a>(200, 50, 0),  <span class="comment">// color (red, green, blue)</span></div><div class="line">      100,                  <span class="comment">// dot size</span></div><div class="line">      75                    <span class="comment">// drawing layer (order of rendering by client)</span></div><div class="line">    ), <span class="keyword">true</span>);</div><div class="line">    <a name="a8"></a><a class="code" href="classArRangeDevice.html#ac5ac42251d038f50250117e7ae21cbdc">setCumulativeDrawingData</a>(<span class="keyword">new</span> <a class="code" href="classArDrawingData.html">ArDrawingData</a>(<span class="stringliteral">&quot;polyDots&quot;</span>, <a class="code" href="classArColor.html">ArColor</a>(100, 20, 0), 120, 74), <span class="keyword">true</span>);</div><div class="line">  }</div><div class="line"></div><div class="line">  <span class="keyword">virtual</span> ~ExampleRangeDevice()</div><div class="line">  {</div><div class="line">  }</div><div class="line"></div><div class="line"></div><div class="line">  <span class="keywordtype">void</span> update()</div><div class="line">  {</div><div class="line">    <a name="a9"></a><a class="code" href="classArRangeDevice.html#a172e8da71d800f959523e1f3021612d8">lockDevice</a>();</div><div class="line"></div><div class="line">    <span class="comment">// Set new fake data in our set of ArSensorReading objects. For a real</span></div><div class="line">    <span class="comment">// sensor, you would have received this data from the sensor driver,</span></div><div class="line">    <span class="comment">// software or hardware itself.</span></div><div class="line">    <span class="comment">//</span></div><div class="line">    <span class="comment">// After updating each ArSensorReading object the detected obstacle position is added to our</span></div><div class="line">    <span class="comment">// ArRangeBuffer objects from the ArRangeDevice class.</span></div><div class="line">    <span class="comment">//</span></div><div class="line">    <span class="comment">// If you have an unknown or variable set of readings, you can resize the </span></div><div class="line">    <span class="comment">// ArSensorReading vector like this:</span></div><div class="line">    <span class="comment">// @code</span></div><div class="line">    <span class="comment">//       myReadings.resize(size);</span></div><div class="line">    <span class="comment">// @endcode</span></div><div class="line">    <span class="comment">// However, new ArSensorReading objects added to the resized vector will be initialized to have an origin position on the</span></div><div class="line">    <span class="comment">// robot of (0,0,0) so you will need to reposition each ArSensorReading in</span></div><div class="line">    <span class="comment">// the vector with resetPosition().</span></div><div class="line"></div><div class="line">    <span class="keywordflow">for</span>(std::vector&lt;ArSensorReading&gt;::iterator i = myReadings.begin(); i != myReadings.end(); ++i)</div><div class="line">    {</div><div class="line">      <span class="comment">// fake range. for a real sensor, use a range value provided by the</span></div><div class="line">      <span class="comment">// sensor:</span></div><div class="line">      <span class="keywordtype">double</span> fakeRange = 5000; <span class="comment">// mm</span></div><div class="line"></div><div class="line">      <span class="comment">// Get current state of robot. In this example, this method (updateTask())</span></div><div class="line">      <span class="comment">// is being called by ArRobot in its task thread. If you were acessing</span></div><div class="line">      <span class="comment">// ArRobot from a different thread instead (main thread, new thread), then</span></div><div class="line">      <span class="comment">// you must lock the robot while accessing it (robot-&gt;lock(),</span></div><div class="line">      <span class="comment">// robot-&gt;unlock()).</span></div><div class="line">      <a name="_a10"></a><a class="code" href="classArRobot.html">ArRobot</a> *robot = <a name="a11"></a><a class="code" href="classArRangeDevice.html#ae70473f04371cb1b4bcac83f1e2cd5a2">getRobot</a>();</div><div class="line">      <a name="_a12"></a><a class="code" href="classArPose.html">ArPose</a> robotPose = robot-&gt;<a name="a13"></a><a class="code" href="group__easy.html#ga25c215578baa598a8916947bdf6e1285">getPose</a>();</div><div class="line">      <a class="code" href="classArPose.html">ArPose</a> robotEncoderPose = robot-&gt;<a name="a14"></a><a class="code" href="classArRobot.html#a6299aaf435ca151f9b15a8cb5bfa9ad3">getEncoderPose</a>();</div><div class="line">      <a name="_a15"></a><a class="code" href="classArTime.html">ArTime</a> currentTime; <span class="comment">// will be set to current system time. You can adjust this if sensor has a constant latency sending the data or provides time delay information.</span></div><div class="line">      <a name="_a16"></a><a class="code" href="classArTransform.html">ArTransform</a> toGlobal = robot-&gt;<a name="a17"></a><a class="code" href="classArRobot.html#adf0187bab7d572ad65be3849b2cd7c96">getToGlobalTransform</a>();</div><div class="line">      ++myCounter;</div><div class="line"></div><div class="line"></div><div class="line">      <span class="comment">// Update the ArSensorReading object </span></div><div class="line">      i-&gt;newData(fakeRange, robotPose, robotEncoderPose, toGlobal, myCounter, currentTime);</div><div class="line"></div><div class="line">      <span class="comment">//add the calculated position (global) to our current and cumulative buffers</span></div><div class="line">      myCurrentBuffer.<a name="a18"></a><a class="code" href="classArRangeBuffer.html#aa5245f84f5fc2bd5f763770ddbeadd5f">addReading</a>(i-&gt;getX(), i-&gt;getY());</div><div class="line">      myCumulativeBuffer.<a class="code" href="classArRangeBuffer.html#aa5245f84f5fc2bd5f763770ddbeadd5f">addReading</a>(i-&gt;getX(), i-&gt;getY());</div><div class="line"></div><div class="line">      <a class="code" href="classArLog.html#a43a8b3789126c818f390f24bdbceccce">ArLog::log</a>(<a class="code" href="classArLog.html#ac8cc0fb3aa323ab2a1c21340fdd1dce3a7040faf60eeb155eaa85d439b1066ca1">ArLog::Normal</a>, <span class="stringliteral">&quot;CustomSensorExample: Updated reading range %f =&gt; (%.2f, %2f)   (update #%u)&quot;</span>, fakeRange, i-&gt;getX(), i-&gt;getY(), myCounter);</div><div class="line">    }</div><div class="line"></div><div class="line">    <a name="a19"></a><a class="code" href="classArRangeDevice.html#af889a070950aae211df4637fbb73608e">unlockDevice</a>();</div><div class="line">  }</div><div class="line">    </div><div class="line">};</div><div class="line"></div><div class="line"></div><div class="line"><span class="keywordtype">int</span> main(<span class="keywordtype">int</span> argc, <span class="keywordtype">char</span> **argv)</div><div class="line">{</div><div class="line">  <a name="a20"></a><a class="code" href="classAria.html#ad99c16b5d947229d9f8e1c5b2d4cdd73">Aria::init</a>();</div><div class="line">  <a name="_a21"></a><a class="code" href="classArArgumentParser.html">ArArgumentParser</a> parser(&amp;argc, argv);</div><div class="line">  parser.<a name="a22"></a><a class="code" href="classArArgumentParser.html#a501694b62a7f8b0c3cadd84aeafe1b35">loadDefaultArguments</a>();</div><div class="line">  <a class="code" href="classArRobot.html">ArRobot</a> robot;</div><div class="line">  ExampleRangeDevice exampleRangeDevice;</div><div class="line"></div><div class="line">  <span class="comment">// Connect to the robot, get some initial data from it such as type and name,</span></div><div class="line">  <span class="comment">// and then load parameter files for this robot.</span></div><div class="line">  <a name="_a23"></a><a class="code" href="classArRobotConnector.html">ArRobotConnector</a> robotConnector(&amp;parser, &amp;robot);</div><div class="line">  <span class="keywordflow">if</span>(!robotConnector.<a name="a24"></a><a class="code" href="classArRobotConnector.html#ab192ac393d9cc1e5b61b1a82b08d085d">connectRobot</a>())</div><div class="line">  {</div><div class="line">    <a class="code" href="classArLog.html#a43a8b3789126c818f390f24bdbceccce">ArLog::log</a>(<a name="a25"></a><a class="code" href="classArLog.html#ac8cc0fb3aa323ab2a1c21340fdd1dce3a012daf6573594f91242f8dd7c02eb74b">ArLog::Terse</a>, <span class="stringliteral">&quot;customRangeSensorExample: Could not connect to the robot.&quot;</span>);</div><div class="line">    <span class="keywordflow">if</span>(parser.<a name="a26"></a><a class="code" href="classArArgumentParser.html#ac3889cbdfa862031c51bf80e06954914">checkHelpAndWarnUnparsed</a>())</div><div class="line">    {</div><div class="line">        <span class="comment">// -help not given</span></div><div class="line">        <a name="a27"></a><a class="code" href="classAria.html#a10d71f3d4d0cf7f38c58a1f749f64a42">Aria::logOptions</a>();</div><div class="line">        <a name="a28"></a><a class="code" href="classAria.html#a6abc3d39b3e9548063bb8e78375acc17">Aria::exit</a>(1);</div><div class="line">    }</div><div class="line">  }</div><div class="line"></div><div class="line">  <span class="keywordflow">if</span> (!<a name="a29"></a><a class="code" href="classAria.html#a1b090c01d88bb420b8cf8e0384d25ee1">Aria::parseArgs</a>() || !parser.<a class="code" href="classArArgumentParser.html#ac3889cbdfa862031c51bf80e06954914">checkHelpAndWarnUnparsed</a>())</div><div class="line">  {</div><div class="line">    <a class="code" href="classAria.html#a10d71f3d4d0cf7f38c58a1f749f64a42">Aria::logOptions</a>();</div><div class="line">    <a class="code" href="classAria.html#a6abc3d39b3e9548063bb8e78375acc17">Aria::exit</a>(1);</div><div class="line">  }</div><div class="line"></div><div class="line">  <a class="code" href="classArLog.html#a43a8b3789126c818f390f24bdbceccce">ArLog::log</a>(<a class="code" href="classArLog.html#ac8cc0fb3aa323ab2a1c21340fdd1dce3a7040faf60eeb155eaa85d439b1066ca1">ArLog::Normal</a>, <span class="stringliteral">&quot;customRangeSensorExample: Connected to robot.&quot;</span>);</div><div class="line"></div><div class="line">  robot.<a name="a30"></a><a class="code" href="classArRobot.html#a97d57b28549fdef851c00749a97cc776">addRangeDevice</a>(&amp;exampleRangeDevice);</div><div class="line"></div><div class="line">  <span class="comment">// For an ArNetworkingServer with an ArServerHandlerDrawings object,</span></div><div class="line">  <span class="comment">// add the range device so that ArNetworking can send the drawings data</span></div><div class="line">  <span class="comment">// to clients such as MobileEyes: </span></div><div class="line">  <span class="comment">// drawings.addRangeDevice(&amp;exampleRangeDevice);</span></div><div class="line"></div><div class="line">  robot.<a name="a31"></a><a class="code" href="group__easy.html#gaa3657bebab0086dd14ed0c8c3f76dc8b">enableMotors</a>();</div><div class="line"></div><div class="line">  <span class="comment">// Start the robot processing cycle running in the background.</span></div><div class="line">  <span class="comment">// True parameter means that if the connection is lost, then the </span></div><div class="line">  <span class="comment">// run loop ends.</span></div><div class="line">  robot.<a name="a32"></a><a class="code" href="classArRobot.html#a3c3d878cfd11a2ef91230005a83bfcaa">runAsync</a>(<span class="keyword">true</span>);</div><div class="line"></div><div class="line">  <span class="keywordflow">while</span>(robot.<a name="a33"></a><a class="code" href="group__easy.html#ga808f67ab5b35165c2605b3002df4b499">isRunning</a>())</div><div class="line">  {</div><div class="line">    <span class="comment">// Print out some data.  We must &quot;lock&quot; the ArRobot object</span></div><div class="line">    <span class="comment">// before calling its methods, and &quot;unlock&quot; when done, to prevent conflicts</span></div><div class="line">    <span class="comment">// with the background thread started by the call to robot.runAsync() above.</span></div><div class="line">    <span class="comment">// See the section on threading in the manual for more about this.</span></div><div class="line">    robot.<a name="a34"></a><a class="code" href="classArRobot.html#a5aecc7ac998c9a31b82fdb904efa8609">lock</a>();</div><div class="line">    <span class="keywordtype">bool</span> soc = robot.<a name="a35"></a><a class="code" href="classArRobot.html#a5b6c74dfe47a9148fc41f43042ccebe4">hasStateOfCharge</a>();</div><div class="line">    <span class="keywordtype">float</span> battv = 0.0;</div><div class="line">    <span class="keywordflow">if</span>(soc)</div><div class="line">      battv = robot.<a name="a36"></a><a class="code" href="classArRobot.html#a072a1abd7fdd59adab416596ac66d440">getStateOfCharge</a>();</div><div class="line">    <span class="keywordflow">else</span></div><div class="line">      battv = robot.<a name="a37"></a><a class="code" href="classArRobot.html#a8d1637a9f3d37e55768d2aa1c8c272d6">getBatteryVoltage</a>();</div><div class="line">    <a class="code" href="classArLog.html#a43a8b3789126c818f390f24bdbceccce">ArLog::log</a>(<a class="code" href="classArLog.html#ac8cc0fb3aa323ab2a1c21340fdd1dce3a7040faf60eeb155eaa85d439b1066ca1">ArLog::Normal</a>, <span class="stringliteral">&quot;customRangeSensorExample: Pose=(%.2f,%.2f,%.2f), Trans.  Vel=%.2f, Battery=%.2f%c&quot;</span>,</div><div class="line">      robot.<a name="a38"></a><a class="code" href="group__easy.html#ga4c994926aaefc4c86380f1a74e508a76">getX</a>(), robot.<a name="a39"></a><a class="code" href="group__easy.html#ga9164db66102fdf9d422feefdadffca92">getY</a>(), robot.<a name="a40"></a><a class="code" href="group__easy.html#gaefe932449c50da9ca0f8084cd9639583">getTh</a>(), robot.<a name="a41"></a><a class="code" href="group__easy.html#gad75dbd9aede7b7f57c9a38902b8aa1ce">getVel</a>(), battv, soc?<span class="charliteral">&#39;%&#39;</span>:<span class="charliteral">&#39;V&#39;</span>);</div><div class="line">    <span class="keywordtype">double</span> angle = 0;</div><div class="line">    <span class="keywordtype">double</span> dist = robot.<a name="a42"></a><a class="code" href="classArRobot.html#af022d2fe8078cd7f9dc82631e40fad13">checkRangeDevicesCurrentPolar</a>(-180, 180, &amp;angle);</div><div class="line">    <a class="code" href="classArLog.html#a43a8b3789126c818f390f24bdbceccce">ArLog::log</a>(<a class="code" href="classArLog.html#ac8cc0fb3aa323ab2a1c21340fdd1dce3a7040faf60eeb155eaa85d439b1066ca1">ArLog::Normal</a>, <span class="stringliteral">&quot;customRangeSensorExample: Closest range reading is %f mm at %f degrees.&quot;</span>, dist, angle);</div><div class="line">    robot.<a name="a43"></a><a class="code" href="classArRobot.html#a403d9de010dc2b8d478cd0b14b2d9491">unlock</a>();</div><div class="line"></div><div class="line">    <span class="comment">// Update the range device.  In a real instance, this could be a callback or</span></div><div class="line">    <span class="comment">// other call from the device driver, library, framework or other software</span></div><div class="line">    <span class="comment">// providing the sensor data, e.g. passing the updated data as an argument.</span></div><div class="line">    exampleRangeDevice.update();</div><div class="line">    </div><div class="line"></div><div class="line">    <span class="comment">// Sleep for 0.5 seconds.</span></div><div class="line">    <a name="a44"></a><a class="code" href="group__easy.html#ga4dfb878a3f9123b8dd96ae05b2fd94cb">ArUtil::sleep</a>(500);</div><div class="line">  }</div><div class="line"></div><div class="line">  </div><div class="line">  <a class="code" href="classArLog.html#a43a8b3789126c818f390f24bdbceccce">ArLog::log</a>(<a class="code" href="classArLog.html#ac8cc0fb3aa323ab2a1c21340fdd1dce3a7040faf60eeb155eaa85d439b1066ca1">ArLog::Normal</a>, <span class="stringliteral">&quot;customRangeSensorExample: Ending robot thread...&quot;</span>);</div><div class="line">  robot.<a name="a45"></a><a class="code" href="group__easy.html#gaff02cf7043c2e7239ec6b6e11ff6b146">stopRunning</a>();</div><div class="line"></div><div class="line">  <span class="comment">// wait for the thread to stop</span></div><div class="line">  robot.<a name="a46"></a><a class="code" href="group__easy.html#ga95201be7c39b521983b51e6f5d9303e4">waitForRunExit</a>();</div><div class="line"></div><div class="line">  <span class="comment">// exit</span></div><div class="line">  <a class="code" href="classArLog.html#a43a8b3789126c818f390f24bdbceccce">ArLog::log</a>(<a class="code" href="classArLog.html#ac8cc0fb3aa323ab2a1c21340fdd1dce3a7040faf60eeb155eaa85d439b1066ca1">ArLog::Normal</a>, <span class="stringliteral">&quot;customRangeSensorExample: Exiting.&quot;</span>);</div><div class="line">  <a class="code" href="classAria.html#a6abc3d39b3e9548063bb8e78375acc17">Aria::exit</a>(0);</div><div class="line">  <span class="keywordflow">return</span> 0;</div><div class="line">}</div></div><!-- fragment --> </div><!-- contents -->
</div><!-- doc-content -->
<!-- start footer part -->
<div id="nav-path" class="navpath"><!-- id is needed for treeview function! -->
  <ul>
    <li class="footer">Generated by
    <a href="http://www.doxygen.org/index.html">
    <img class="footer" src="doxygen.png" alt="doxygen"/></a> 1.8.11 </li>
  </ul>
</div>
</body>
</html>
